\section*{\large Design}
\begin{normalsize}

Figure \ref{fig:workflow} displays where our application, namely the node filtering, is located in the workflow. Basically, it subscribes the \emph{topic joint\_states\_raw} and publishes to the topic \emph{joint\_states}, the messages involved are of type \emph{sensor\_msgs/JointState.msg} and hold the state of every joint.

\begin{figure}[!htb]
\centering
\includegraphics[scale=0.5]{./images/workflow.png}
\caption{the work flow \label{fig:workflow}}
\end{figure}

The node filtering acts as a man in the middle in order to correct for the imprecision of the skeleton. Optionally, it can instantiate an object of the CoMUtilities class in order to calculate and visualize the center of mass instant after instant. The visualization is implemented by three methods: 
\begin{enumerate}
\item \emph{showComX()}, which prints the position of the robot CoM with respect to the x-axis of the support polygon, see the terminal in Figure \ref{fig:com};
\item \emph{visualize\_links\_coms(pcl::PointCloud$<$pcl::PointXYZ$>$::Ptr coms)}, which adds the point cloud of the CoM of the links in the PCL visualizer, see the white points in Figure \ref{fig:com};
\item \emph{visualize\_com(pcl::PointCloud$<$pcl::PointXYZ$>$::Ptr com\_pcl)}, which adds the CoM of the robot and its projection in the support polygon in the PCL visualizer, see the green points in Figure \ref{fig:com};
\end{enumerate}

\begin{figure}[!htb]
\centering
\includegraphics[scale=0.35]{./images/com.png}
\caption{center of mass utilities \label{fig:com}}
\end{figure}

The CoM is calculated according to the following formulas, all the necessary is contained in the URDF model:

\begin{align*}
CoM_x = \sum_{i = 1}^{N} m_i x_i / M \text{, }
CoM_y = \sum_{i = 1}^{N} m_i y_i / M \text{, }
CoM_z = \sum_{i = 1}^{N} m_i z_i / M \text{, where }
\end{align*}

$N = 18$ is the number of links, $m_i$ is the $i^{th}$ link mass, M is the total mass and $x_i$, $y_i$, $z_i$ are the coordinates of the $i^{th}$ link with respect to the \emph{right\_foot} plus the translations from the local reference frames. We chose the \emph{right\_foot} (or \emph{left\_foot}) as the reference frame for the support polygon and for the TF calculations since it stays on the ground and the x-axis is parallel to it even if the legs are spread. We did not choose the torso because it does not stay on the ground if, for example, the robot tilts forward or backward, as can be seen in RVIZ. However, as a result of the nature of the main problems we encountered, its computation did not prove to be useful.

\end{normalsize}